/**
 * 四轮麦轮底盘
 *
 * 以码盘正方向为Y轴正方向，右侧为X轴正方向
 * -----右前ID为1----- *
 * -----左前ID为2----- *
 * -----左后ID为3----- *
 * -----右后ID为4----- *
 */

#ifndef __MECANUMCHASSIS_H
#define __MECANUMCHASSIS_H

#include "main.h"
#include "PID.h"

#define RADIUS 0.152f // 轮子直径
#define LENGTH 0.130f // 底盘半长
#define WIDTH 0.188f  // 底盘半宽
#define RR 0.22857f   // 轮子到中心的距离
#define DECRATIO 19   // 电机减速比

struct chassis_t
{
    int16_t Motors_Speed[4];

    float roll;  // 横滚角， 绕x轴
    float pitch; // 俯仰角，绕y轴
    float yaw;   // 偏航角， 绕z轴

    PID_t pid; // 角度pid
} chassis;

void Chassis_Init(void);
void Chassis_SetSpeed(float Vx, float Vy, float Vw);
void Chassis_SetAngle(float angle);
void Chassis_SetPoint(float Px, float Py, float angle);

#endif
